#include "daq_poc.h"

int main(
	) {

	initParams();
	setupDevs();

	while (!SW_TERMINATE) {
		startDevs();
		#pragma omp parallel
		{
			int32_t tid;
			tid = omp_get_thread_num();
			if (tid == 0)
				while (SW_ACQ_LIVE)
					maintainBuffers();
			else {
				Sleep((int)(CAP_LEN*1000));
				waitForKey();
			}
		}
		stopDevs();
		writeFile();
	}

	killDevs();

	return EXIT_SUCCESS;
}

void initParams(
	) {
	/* SET PARAMS */ {
		PARAM.capture_length = CAP_LEN;
		PARAM.force_sample_rate = DAQ_HZ;
		PARAM.video_frame_rate = VID_FPS;
	}
	/* SANITY CHECK */ {
		assert(PARAM.capture_length * PARAM.video_frame_rate <= 8800);
		assert(PARAM.force_sample_rate * DAQ_DEV_N_CHAN <= 48000);
	}

	return;
}
void setupDevs(
	) {
	/* DAQ */ {
		int32_t	err = 0;
		uInt32 	serial1, serial2;

		err = DAQmxGetDevSerialNum ("Dev1", &serial1);
		if( DAQmxFailed(err) ) diagDAQ(err);
		err = DAQmxGetDevSerialNum ("Dev2", &serial2);
		if( DAQmxFailed(err) ) diagDAQ(err);
		assert (serial1 != serial2);

		if (serial1 == DAQ_L_SERIAL)
			DEV.daq_chan_L = "Dev1/ai0, Dev1/ai1, Dev1/ai2";
		else if (serial1 == DAQ_R_SERIAL)
			DEV.daq_chan_R = "Dev1/ai0, Dev1/ai1, Dev1/ai2";
		if (serial2 == DAQ_L_SERIAL)
			DEV.daq_chan_L = "Dev2/ai0, Dev2/ai1, Dev2/ai2";
		else if (serial2 == DAQ_R_SERIAL)
			DEV.daq_chan_R = "Dev2/ai0, Dev2/ai1, Dev2/ai2";
	}
	/* CAM */ {
		CAM_RC 	rc;
		int32_t ndev;
		char*	camNames[10];
		camNames[0] = new char[40];

		rc = DEV.video.GetCamNames(ndev,camNames);
		assert(rc == CAM_RC_SUCCESS);
		assert(ndev > 0);
		rc = DEV.video.InitCam(0);
		assert(rc == CAM_RC_SUCCESS);

		DEV.video.GetCamParms()->p.Fps = PARAM.video_frame_rate;
		DEV.video.GetCamParms()->p.SizeX = VID_W;
		DEV.video.GetCamParms()->p.SizeY = VID_H;
		DEV.video.GetCamParms()->p.PreTrigFrames = VID_LEN;
		DEV.video.GetCamParms()->p.SequenceLength = VID_LEN;

		rc = DEV.video.SetupCam();
		assert(rc == CAM_RC_SUCCESS);
	}
	return;
}
void startDevs(
	) {
	/* DAQ */ {
		int32 err;

		err = DAQmxCreateTask("", &DEV.force_L);
		if( DAQmxFailed(err) ) diagDAQ(err);
		err = DAQmxCreateTask("", &DEV.force_R);
		if( DAQmxFailed(err) ) diagDAQ(err);

		err = DAQmxCreateAIVoltageChan(
			DEV.force_L, DEV.daq_chan_L, "", DAQmx_Val_Cfg_Default, 
			-DAQ_VOLT_RANGE, DAQ_VOLT_RANGE, DAQmx_Val_Volts, NULL
			);
		if( DAQmxFailed(err) ) diagDAQ(err);
		err = DAQmxCreateAIVoltageChan(
			DEV.force_R, DEV.daq_chan_R, "", DAQmx_Val_Cfg_Default, 
			-DAQ_VOLT_RANGE, DAQ_VOLT_RANGE, DAQmx_Val_Volts, NULL
			);
		if( DAQmxFailed(err) ) diagDAQ(err);

		err = DAQmxCfgSampClkTiming(
			DEV.force_L, "OnboardClock", PARAM.force_sample_rate, 
			DAQmx_Val_Rising, DAQmx_Val_ContSamps, 0
			);
		if( DAQmxFailed(err) ) diagDAQ(err);
		err = DAQmxCfgSampClkTiming(
			DEV.force_R, "OnboardClock", PARAM.force_sample_rate, 
			DAQmx_Val_Rising, DAQmx_Val_ContSamps, 0
			);
		if( DAQmxFailed(err) ) diagDAQ(err);

		err = DAQmxCfgInputBuffer(DEV.force_L, 200000);
		if( DAQmxFailed(err) ) diagDAQ(err);
		err = DAQmxCfgInputBuffer(DEV.force_R, 200000);
		if( DAQmxFailed(err) ) diagDAQ(err);

		err = DAQmxStartTask(DEV.force_L);
		if( DAQmxFailed(err) ) diagDAQ(err);
		err = DAQmxStartTask(DEV.force_R);
		if( DAQmxFailed(err) ) diagDAQ(err);

		BUFFER.force_ptr = 0;
	}
	/* CAM */ {
		CAM_RC rc;

		rc = DEV.video.SetMode(CAM_REC);
			assert (rc == CAM_RC_SUCCESS);
		DEV.video.Trig1();

		BUFFER.video_ptr = 0;
	}

	SW_ACQ_LIVE = 1;

	return;
}
void maintainBuffers(
	) {
	int32 err;
	int32 pointsRead;

	err = DAQmxReadAnalogF64(
		DEV.force_L, 				// source
		DAQ_BLOCK_LEN/DAQ_DEV_N_CHAN,		// n_samps/CHAN
		1,					// timeout
		DAQmx_Val_GroupByScanNumber, 		// fillMode
		&*(BUFFER.force_L + BUFFER.force_ptr), 	// &write
		DAQ_N_BLOCKS*DAQ_BLOCK_LEN,		// writeSize(samps)
		&pointsRead, NULL			// etc
		);
	if( DAQmxFailed(err) ) diagDAQ(err);
	
	err = DAQmxReadAnalogF64(
		DEV.force_R, 				// source
		DAQ_BLOCK_LEN/DAQ_DEV_N_CHAN,		// n_samps/CHAN
		1,					// timeout
		DAQmx_Val_GroupByScanNumber, 		// fillMode
		&*(BUFFER.force_R + BUFFER.force_ptr), 	// &write
		DAQ_N_BLOCKS*DAQ_BLOCK_LEN,		// writeSize(samps)
		&pointsRead, NULL			// etc
		);
	if( DAQmxFailed(err) ) diagDAQ(err);

	/* ROTATE BUFFER */ {
		assert (pointsRead == DAQ_BLOCK_LEN/DAQ_DEV_N_CHAN);

		BUFFER.force_ptr += pointsRead;
		if (BUFFER.force_ptr > DAQ_BLOCK_LEN*DAQ_N_BLOCKS) {
			for (int i = 0; i < DAQ_BLOCK_LEN; i++) {
				BUFFER.force_L[BUFFER.force_ptr - DAQ_SIG_LEN - DAQ_BLOCK_LEN + i] 
				=
				BUFFER.force_L[BUFFER.force_ptr - DAQ_BLOCK_LEN + i];

				BUFFER.force_R[BUFFER.force_ptr - DAQ_SIG_LEN - DAQ_BLOCK_LEN + i] 
				=
				BUFFER.force_R[BUFFER.force_ptr - DAQ_BLOCK_LEN + i];
			}
		}

		if (BUFFER.force_ptr >= DAQ_BUFF_LEN - DAQ_BLOCK_LEN) {
			for (int i = 0; i < DAQ_BLOCK_LEN; i++) {
				BUFFER.force_L[BUFFER.force_ptr - DAQ_SIG_LEN + i] 
				=
				BUFFER.force_L[BUFFER.force_ptr + i];

				BUFFER.force_R[BUFFER.force_ptr - DAQ_SIG_LEN + i] 
				=
				BUFFER.force_R[BUFFER.force_ptr + i];
			}
			BUFFER.force_ptr = DAQ_SIG_LEN - DAQ_BLOCK_LEN;
		}
	}

	return;
}
void waitForKey(
	) {
	char entered;

	printf("Press enter to save last %f seconds.\n\t"
		"Pressing q or Q then enter saves capture and exits.\n"
		, PARAM.capture_length);
	entered = getchar();

	SW_ACQ_LIVE = 0;
	if (entered == 'q' || entered == 'Q')
		SW_TERMINATE = 1;

	return;
}
void stopDevs(
	) {
	/* DAQ */ {
		int32 err;

		err = DAQmxStopTask(DEV.force_L);
		if( DAQmxFailed(err) ) diagDAQ(err);
		err = DAQmxStopTask(DEV.force_R);
		if( DAQmxFailed(err) ) diagDAQ(err);

		err = DAQmxClearTask(DEV.force_L);
		if( DAQmxFailed(err) ) diagDAQ(err);
		err = DAQmxClearTask(DEV.force_R);
		if( DAQmxFailed(err) ) diagDAQ(err);
	}
	/* CAM */ {
		CAM_RC rc;

		DEV.video.Trig2();
		rc = DEV.video.SetMode(CAM_DISK);
			assert (rc == CAM_RC_SUCCESS);

		int8_t*	image;
		int32_t video_ptr = 0;
		for (int i = 0; i < VID_LEN; i++) {
			DEV.video.GetFrame(0,i);
			image = (int8_t*)DEV.video.GetDispPixels();
			for (int x = 0; x < VID_H*VID_W; x++)
				BUFFER.video[video_ptr++] = (int8_t)image[x];
		}
	}

	printf("Data in RAM...\n");
	return;
}
void writeFile(
	) {
	static int32_t trial_number = 0;
	char filename[256];

	/* FORCE */ {
		FILE*	L_X;
		FILE*	L_Y;
		FILE*	L_Z;
		FILE*	R_X;
		FILE*	R_Y;
		FILE*	R_Z;

		sprintf(filename, "data/L_X_%i.dat", trial_number);
		L_X = fopen(filename, "w");
		sprintf(filename, "data/L_Y_%i.dat", trial_number);
		L_Y = fopen(filename, "w");
		sprintf(filename, "data/L_Z_%i.dat", trial_number);
		L_Z = fopen(filename, "w");
		sprintf(filename, "data/R_X_%i.dat", trial_number);
		R_X = fopen(filename, "w");
		sprintf(filename, "data/R_Y_%i.dat", trial_number);
		R_Y = fopen(filename, "w");
		sprintf(filename, "data/R_Z_%i.dat", trial_number);
		R_Z = fopen(filename, "w");

		for (int i = 0; i < DAQ_SIG_LEN; i += 3) {
			fprintf(L_Z, "%lf\n", BUFFER.force_L[BUFFER.force_ptr - DAQ_SIG_LEN + i + 0]);
			fprintf(L_X, "%lf\n", BUFFER.force_L[BUFFER.force_ptr - DAQ_SIG_LEN + i + 1]);
			fprintf(L_Y, "%lf\n", BUFFER.force_L[BUFFER.force_ptr - DAQ_SIG_LEN + i + 2]);
			fprintf(R_Z, "%lf\n", BUFFER.force_R[BUFFER.force_ptr - DAQ_SIG_LEN + i + 0]);
			fprintf(R_X, "%lf\n", BUFFER.force_R[BUFFER.force_ptr - DAQ_SIG_LEN + i + 1]);
			fprintf(R_Y, "%lf\n", BUFFER.force_R[BUFFER.force_ptr - DAQ_SIG_LEN + i + 2]);
		}

		fclose(L_X);
		fclose(L_Y);
		fclose(L_Z);
		fclose(R_X);
		fclose(R_Y);
		fclose(R_Z);
	}
	/* VIDEO */ {
		CvVideoWriter*	VID;
		sprintf(filename, "data/VID_%i.avi", trial_number);
		VID = cvCreateVideoWriter(filename, -1, 30.0, cvSize(VID_W, VID_H), 0);

		IplImage IMG; {
			IMG.nSize = 		sizeof(IplImage);
			IMG.ID = 		0;
			IMG.nChannels = 	1;
			IMG.depth = 		IPL_DEPTH_8U;
			IMG.dataOrder = 	0;
			IMG.origin =		0;
			IMG.width = 		VID_W;
			IMG.height = 		VID_H;
			IMG.roi =		NULL;
			IMG.maskROI =		NULL;
			IMG.tileInfo =		NULL;
			IMG.imageSize = 	VID_W*VID_H;
			IMG.widthStep = 	VID_W;
		}

		int32_t video_ptr = 0;
		for (int i = 0; i < VID_LEN; i++) {
			IMG.imageData = 	(char*)&BUFFER.video[video_ptr];
			IMG.imageDataOrigin = 	(char*)&BUFFER.video[video_ptr];
			video_ptr += VID_H*VID_W;
			cvWriteFrame(VID, &IMG);
		}

		cvReleaseVideoWriter(&VID);
	}

	trial_number++;
	printf("Data saved to disk.\n");
	return;
}
void killDevs(
	) {
	/* DAQ */ {
	}
	/* CAM */ {
		DEV.video.SetMode(CAM_IDLE);
	}
	return;
}
void diagDAQ(
	int 	err
	) {
	char errBuf[2048] = {'\0'};

	DAQmxGetExtendedErrorInfo(errBuf,2048);
	if( DAQmxFailed(err) ) {
		printf ("DAQmx Error %i: %s\n", err, errBuf);
		printf ("daq_poc: Exiting Program...\n");
	}
	exit(EXIT_FAILURE);
}
